/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * $Id: main.c 46 2014-04-22 04:40:46Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <stdio.h>

// Maxim CMSIS includes
#include "config.h"
#include "errors.h"
#include "bsp.h"
#include "i2c.h"
#include "fb_console.h"

// RTOS includes
#include "FreeRTOS.h"
#include "task.h"

// Other includes
#include "car.h"
#include "compass.h"
#include "motor.h"
#include "encoder.h"

/***** Definitions *****/

/***** Global Variables *****/
unsigned int compass_exists;

/***** Function Prototypes *****/
extern int userif_init(void);

//******************************************************************************
int main(void)
{
  if (bsp_init() != E_NO_ERROR) {
    printf("ERROR: bsp_init() failed\n");
    while(1);
  }

  // Initialize the frame buffer console
  fb_init();

  printf("\n\n***** Singularity *****\n");

  if (userif_init() != E_NO_ERROR) {
    printf("ERROR: userif_init() failed\n");
    while(1);
  }

  if (i2c_init((void*)MXC_BASE_I2C, I2C_STD_MODE) != E_NO_ERROR) {
    printf("ERROR: i2c_init() failed\n");
    while(1);
  }

  if (enc_init() != E_NO_ERROR) {
    printf("ERROR: enc_init() failed\n");
    while(1);
  }

  if (motor_init() != E_NO_ERROR) {
    printf("ERROR: motor_init() failed\n");
    while(1);
  }

  if (compass_init() != E_NO_ERROR) {
    compass_exists = 0;
    printf("ERROR: compass_init() failed\n");
  } else {
    compass_exists = 1;
  }
  
  // Start the RTOS scheduler
  vTaskStartScheduler();

  /* Will only get here if there was insufficient memory to create the idle task. */
  while(1);

  return 0;
}

//*****************************************************************************
void vApplicationTickHook(void)
{
  static unsigned long ulTickCounter;

  // Toggle the LED every second
  ulTickCounter++;
  if ((ulTickCounter % configTICK_RATE_HZ) == 0) {
    led_toggle(0);
  }
}

//*****************************************************************************
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName)
{
  printf("RTOS App Stack Overflow (\"%s\")\n", pcTaskName);
  while(1);
}

